// algorithms.go
package Demodulation

import "math"

func NewDemoQPSK(param DemodulationInitParameter) *DemoQPSK {
	d := &DemoQPSK{
		rb:         param.Rb,
		fs:         param.Fs,
		sampPerSym: 4,
		sliceSize:  8192,
		firstFrame: true,
		lastSymbol: 0,
	}

	// 1. 根升余弦滤波器（9 抽头，对称）
	coef := []float32{
		-0.042463605937143, 1.462582099439346e-17, 0.212318029685715,
		0.500262571713977, 0.636954089057146, 0.500262571713977,
		0.212318029685715, 1.462582099439346e-17, -0.042463605937143,
	}
	d.blockFilter = &BlockFilter{
		Coef:    coef,
		Taps:    9,
		EvenSym: true,
		Delay:   make([]Complex, 9),
	}

	// 2. 符号同步参数（Gardner）
	d.loopF1 = 0.003332997710206 / 10
	d.loopF2 = 5.556114814792136e-06 / 100
	d.symSyncBuf = &SymbolSyncBuffer{
		W: 0.5, N: 0.9, U: 0.6,
	}

	// 3. 三段 Costas PLL（已修复原 C++ 第三段写错 bug）
	d.setupCostasPLL(param.Rb)

	return d
}

func (d *DemoQPSK) setupCostasPLL(rb int) {
	sigma := 0.707
	K := 1.0
	Tn := 1.0 / (4.0 * float64(rb))

	BnCoeffs := []float64{0.03, 0.03, 0.02} // 三段带宽系数
	for i := 0; i < 3; i++ {
		BL := BnCoeffs[i] * float64(rb)
		Wn := 8 * sigma * BL / (4*sigma*sigma + 1)
		d.pllStages[i].Coef1 = float32(2 * sigma * Wn * Tn / K)
		d.pllStages[i].Coef2 = float32((Wn * Tn) * (Wn * Tn) / K)
	}
}

// 块滤波（对称系数优化版）
func (d *DemoQPSK) BlockFilter(in, out []Complex) {
	f := d.blockFilter
	f.mu.Lock()
	defer f.mu.Unlock()

	copy(f.Delay[0:], f.Delay[1:])
	f.Delay[8] = in[len(in)-1] // 先预存最后一个，循环中再逐个移

	for i, sample := range in {
		copy(f.Delay[0:8], f.Delay[1:9])
		f.Delay[8] = sample

		var sumI, sumQ float32
		for j := 0; j < 4; j++ {
			sumI += (f.Delay[j].I + f.Delay[8-j].I) * f.Coef[j]
			sumQ += (f.Delay[j].Q + f.Delay[8-j].Q) * f.Coef[j]
		}
		sumI += f.Delay[4].I * f.Coef[4]
		sumQ += f.Delay[4].Q * f.Coef[4]

		out[i] = Complex{sumI, sumQ}
	}
}

// 简易 AGC（目标功率 1.0）
func (d *DemoQPSK) AGC(data []Complex) {
	var powSum float64
	for _, c := range data {
		powSum += float64(c.I*c.I + c.Q*c.Q)
	}
	meanPow := powSum / float64(len(data))
	if meanPow < 1e-20 {
		return
	}
	gain := float32(math.Sqrt(1.0 / meanPow))
	for i := range data {
		data[i].I *= gain
		data[i].Q *= gain
	}
}

// Costas 载波恢复（自动三段切换）
func (d *DemoQPSK) CostasPLL(in, out []Complex) {
	stage := 0
	totalSamples := d.SampleCount
	if totalSamples >= 50000 {
		stage = 2
	} else if totalSamples >= 30000 {
		stage = 1
	}
	d.currentStage = stage

	c1 := d.pllStages[stage].Coef1
	c2 := d.pllStages[stage].Coef2

	for i := range in {
		cosP := float32(math.Cos(float64(d.ncoPhase)))
		sinP := float32(math.Sin(float64(d.ncoPhase)))

		I := in[i].I*cosP + in[i].Q*sinP
		Q := in[i].Q*cosP - in[i].I*sinP

		signI := float32(1)
		if I < 0 {
			signI = -1
		}
		signQ := float32(1)
		if Q < 0 {
			signQ = -1
		}

		err := signI*Q - signQ*I
		d.freqIntegral += err * c2
		inc := err*c1 + d.freqIntegral

		d.ncoPhase += inc * 2 * Pi
		for d.ncoPhase > 2*Pi {
			d.ncoPhase -= 2 * Pi
		}

		out[i] = Complex{I, Q}
	}
}

// Gardner + 立方插值符号同步
func (d *DemoQPSK) SymbolSync(in []Complex, out []Complex) (nOut int) {
	s := d.symSyncBuf
	nOut = 0

	for _, sample := range in {
		// 输入缓冲移位
		s.Buff[0] = s.Buff[1]
		s.Buff[1] = s.Buff[2]
		s.Buff[2] = s.Buff[3]
		s.Buff[3] = sample

		s.PastN = s.N
		s.PastW = s.W
		s.NTemp = s.N - s.W
		if s.NTemp < 0 {
			s.N = s.NTemp + 1
		} else {
			s.N = s.NTemp
		}

		// 立方插值
		y := d.cubicInterp(s.Buff, s.U)

		// 每两个样点判断是否抽取
		if (s.KK+1)%2 == 0 {
			out = append(out, y)
			nOut++

			// Gardner 误差检测
			Ia := (y.I + s.YBuff[0].I) / 2
			Qa := (y.Q + s.YBuff[0].Q) / 2
			err := (s.YBuff[1].I-Ia)*(y.I-s.YBuff[0].I) + (s.YBuff[1].Q-Qa)*(y.Q-s.YBuff[0].Q)

			s.Err1 = s.Err2
			s.Err2 = err
			s.W = s.PastW + d.loopF1*(s.Err2-s.Err1) + d.loopF2*s.Err2
		}

		// 更新 Y 缓冲
		s.YBuff[0] = s.YBuff[1]
		s.YBuff[1] = s.YBuff[2]
		s.YBuff[2] = y

		s.U = s.PastN / s.PastW
		s.KK++
	}
	return nOut
}

func (d *DemoQPSK) cubicInterp(x [4]Complex, mu float32) Complex {
	c0I := x[1].I
	c1I := (-x[0].I + x[2].I - x[3].I) / 3
	c2I := 2*x[0].I - 5*x[1].I + 4*x[2].I - x[3].I
	c3I := (-x[0].I + 3*x[1].I - 3*x[2].I + x[3].I) / 2

	c0Q := x[1].Q
	c1Q := (-x[0].Q + x[2].Q - x[3].Q) / 3
	c2Q := 2*x[0].Q - 5*x[1].Q + 4*x[2].Q - x[3].Q
	c3Q := (-x[0].Q + 3*x[1].Q - 3*x[2].Q + x[3].Q) / 2

	mu2 := mu * mu
	I := c0I + c1I*mu + c2I*mu2 + c3I*mu*mu2
	Q := c0Q + c1Q*mu + c2Q*mu2 + c3Q*mu*mu2
	return Complex{I, Q}
}

// 差分硬判决 + 解差分（已彻底解决首次随机问题）
func (d *DemoQPSK) DiffJudgment(symbols []Complex) []byte {
	bits := make([]byte, len(symbols))
	prev := d.lastSymbol

	for i, s := range symbols {
		cur := 0
		if s.I >= 0 && s.Q >= 0 {
			cur = 0
		}
		if s.I < 0 && s.Q >= 0 {
			cur = 3
		}
		if s.I < 0 && s.Q < 0 {
			cur = 2
		}
		if s.I >= 0 && s.Q < 0 {
			cur = 1
		}

		diff := (cur - prev + 4) % 4
		bits[i] = byte(diff) // 0,1,2,3 对应 00,01,11,10（格雷码）

		prev = cur
	}
	if len(symbols) > 0 {
		d.lastSymbol = prev
	}
	return bits
}
